\documentclass[12pt,a4paper]{article}

\include{preamble}

\begin{document}

\maketitle

\noindent
\setlength{\parindent}{2em}
\setlength{\parskip}{0.3em}

\section*{第一题}

已知S-MSCKF中，IMU误差状态量和噪声向量分别为

\begin{equation}
\tilde{\mathbf{x}}_{I} = 
\left(
{}^I_G \tilde{\boldsymbol{\theta}}^\top \quad 
\tilde{\mathbf{b}}_g^\top \quad 
{}^G\tilde{\mathbf{v}}^\top_I \quad 
\tilde{\mathbf{b}}_a^\top \quad
{}^G\tilde{\mathbf{p}}^\top_I \quad
{}^I_C \tilde{\boldsymbol{\theta}}^\top \quad
{}^I\tilde{\mathbf{p}}^\top_C
\right)^\top 
\in \mathbb{R}^{21 \times 1}
\end{equation}

\begin{equation}
\mathbf{n}_I^\top = 
\left(\mathbf{n}_g^\top \; \mathbf{n}_{wg}^\top \; \mathbf{n}_a^\top \; \mathbf{n}_{wa}^\top\right)^\top
\in \mathbb{R}^{12 \times 1}
\end{equation}

根据ESKF中 \textbf{5.3.3 The error-state kinematics} 小节公式

\begin{equation}
\label{equ:imu_diff_func_error_state}
\dot{\delta \mathbf{x}}_{I} = 
\begin{cases}
\begin{aligned}
\dot{\delta \mathbf{p}} &= \delta \mathbf{v} \\
\dot{\delta \mathbf{v}} &= -\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{ \times} \delta \boldsymbol{\theta}-\mathbf{R} \delta \mathbf{a}_{b}+\delta \mathbf{g}-\mathbf{R} \mathbf{a}_{n} \\
\dot{\delta \boldsymbol{\theta}} &= -\left[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right]_{ \times} \delta \boldsymbol{\theta}-\delta \boldsymbol{\omega}_{b}-\boldsymbol{\omega}_{n} \\
\delta \dot{\mathbf{a}}_{b} &= \mathbf{a}_{w} \\ 
\delta \dot{\boldsymbol{\omega}}_{b} &= \boldsymbol{\omega}_{w}
\end{aligned}
\end{cases}
\end{equation}

对式~\eqref{equ:imu_diff_func_error_state}线性化，可得到IMU连续形式误差状态方程

\begin{equation}
\dot{\tilde{\mathbf{x}}}_I = 
\mathbf{F} \tilde{\mathbf{x}}_I + 
\mathbf{G} \mathbf{n}_I
\end{equation}

请求解$F$和$G$。\newline 

答：\newline 

\begin{equation*}
\mathbf{F}_{21 \times 21} = 
\frac{\partial \dot{\tilde{\mathbf{x}}}_I}{\partial \tilde{\mathbf{x}}_I} = 
\begin{pmatrix}
-\left[\boldsymbol{\omega}_{m}-\boldsymbol{\omega}_{b}\right]_{ \times} & -\mathbf{I}_3 & 
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & 
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\
-\mathbf{R}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{ \times} & 
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & 
-\mathbf{R} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & 
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{I}_3 & 
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & 
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & 
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3}
\end{pmatrix}
\end{equation*}

\begin{equation*}
\mathbf{G}_{21 \times 12} = 
\frac{\partial \dot{\tilde{\mathbf{x}}}_I}{\partial \mathbf{n}_I} = 
\begin{pmatrix}
-\mathbf{I}_3 & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\
\mathbf{0}_{3\times 3} & \mathbf{I}_3 &  \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & -\mathbf{R} & \mathbf{0}_{3\times 3} \\
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{I}_3 \\
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} \\
\mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3} & \mathbf{0}_{3\times 3}
\end{pmatrix}
\end{equation*}

\bibliographystyle{unsrt}
\bibliography{bibfile}

\end{document}

